/*
 * @Author: cooper
 * @Date: 2025-11-08 22:39:33
 * @Wechat: cmd_develop
 * @Description:
 */
#include <Arduino.h>

// PWM输出引脚定义
int pwmA = 25;
int pwmB = 32;
int pwmC = 33;
int pwmEN = 26;

// 宏定义实现的一个约束函数,用于限制一个值的范围。
#define _CLAMP(a, min, max) ((a) < (min) ? (min) : ((a) > (max) ? (max) : (a)))
#define MOTO_EN(x) digitalWrite(pwmEN, x)

float voltage_power_supply = 12; // 供电电压
float shaft_angle = 0;           // 机械角度
float open_loop_timestamp = 0;   //
float zero_electric_angle = 0;   // 电角度
float Ualpha, Ubeta = 0;         // 克拉克转换的电压
float Ua, Ub, Uc = 0;            // 三相的电压
float dc_a, dc_b, dc_c = 0;      //
void setup()
{
    Serial.begin(115200);

    // PWM设置
    pinMode(pwmA, OUTPUT);
    pinMode(pwmB, OUTPUT);
    pinMode(pwmC, OUTPUT);
    pinMode(pwmEN, OUTPUT);
    // ESP32中，ledcSetup() 是一个用于配置 LED 控制的函数
    ledcSetup(0, 1000, 8); // pwm频道(channel:0-15), 频率Hz, 精度(8位,10位,12位)
    ledcSetup(1, 1000, 8); // pwm频道(channel:0-15), 频率Hz, 精度(8位,10位,12位)
    ledcSetup(2, 1000, 8); // pwm频道(channel:0-15), 频率Hz, 精度(8位,10位,12位)

    ledcAttachPin(pwmA, 0); // 将通道 0 附加到 pwmA 引脚
    ledcAttachPin(pwmB, 1); // 将通道 1 附加到 pwmB 引脚
    ledcAttachPin(pwmC, 2); // 将通道 2 附加到 pwmC 引脚

    MOTO_EN(1); // 使能PWM

    Serial.println("finish init");
    delay(3000);
}

// 电角度求解
float electrical_angle(float shaft_angle, int pole_pairs)
{
    return (shaft_angle * pole_pairs);
}

// 归一化角度到 [0,2PI]
float normalize_angle(float angle)
{
    // fmod 函数的余数的符号与除数相同。浮点数据
    float a = fmod(angle, 2 * PI); // 取余运算可以用于归一化，列出特殊值例子算便知
    return a >= 0 ? a : (a + 2 * PI);
}

// 设置PWM到控制器输出 void setPwm(float Ua, float Ub, float Uc)
void setPwm(float Ua, float Ub, float Uc)
{
    // 计算占空比
    // 限制占空比从0到1
    dc_a = _CLAMP(Ua / voltage_power_supply, 0.0f, 1.0f);
    dc_b = _CLAMP(Ub / voltage_power_supply, 0.0f, 1.0f);
    dc_c = _CLAMP(Uc / voltage_power_supply, 0.0f, 1.0f);

    // 写入PWM到PWM 0 1 2 通道
    ledcWrite(0, dc_a * 255);
    ledcWrite(1, dc_b * 255);
    ledcWrite(2, dc_c * 255);
}

void setPhaseVoltage(float Uq, float Ud, float angle_el)
{
    angle_el = normalize_angle(angle_el + zero_electric_angle);
    // 帕克逆变换
    Ualpha = -Uq * sin(angle_el); //-Uq * sin(angle_el) + Ud * cos(angle_el)
    Ubeta = Uq * cos(angle_el);   // Uq * cos(angle_el) + Ud * sin(angle_el)

    // 克拉克逆变换
    Ua = Ualpha + voltage_power_supply / 2;
    Ub = (sqrt(3) * Ubeta - Ualpha) / 2 + voltage_power_supply / 2;
    Uc = (-Ualpha - sqrt(3) * Ubeta) / 2 + voltage_power_supply / 2;
    Serial.println(Ua);
    setPwm(Ua, Ub, Uc);
}

// 开环速度函数
float velocityOpenloop(float target_velocity)
{
    unsigned long now_us = micros(); // 获取从开启芯片以来的微秒数，它的精度是 4 微秒。 micros() 返回的是一个无符号长整型（unsigned long）的值
                                     // 计算当前每个Loop的运行时间间隔
    float Ts = (now_us - open_loop_timestamp) * 1e-6f;

    // 由于 micros() 函数返回的时间戳会在大约 70 分钟之后重新开始计数，在由70分钟跳变到0时，TS会出现异常，因此需要进行修正。如果时间间隔小于等于零或大于 0.5 秒，则将其设置为一个较小的默认值，即 1e-3f
    if (Ts <= 0 || Ts > 0.5f)
        Ts = 1e-3f;
    // 通过乘以时间间隔和目标速度来计算需要转动的机械角度，存储在 shaft_angle 变量中。在此之前，还需要对轴角度进行归一化，以确保其值在 0 到 2π 之间。
    shaft_angle = normalize_angle(shaft_angle + target_velocity * Ts);
    // 以目标速度为 10 rad/s 为例，如果时间间隔是 1 秒，则在每个循环中需要增加 10 * 1 = 10 弧度的角度变化量，才能使电机转动到目标速度。
    // 如果时间间隔是 0.1 秒，那么在每个循环中需要增加的角度变化量就是 10 * 0.1 = 1 弧度，才能实现相同的目标速度。因此，电机轴的转动角度取决于目标速度和时间间隔的乘积。

    // 使用早前设置的voltage_power_supply的1/3作为Uq值，这个值会直接影响输出力矩
    // 最大只能设置为Uq = voltage_power_supply/2，否则ua,ub,uc会超出供电电压限幅
    float Uq = voltage_power_supply / 3;

    setPhaseVoltage(Uq, 0, electrical_angle(shaft_angle, 11));

    open_loop_timestamp = now_us; // 用于计算下一个时间间隔
    return Uq;
}

void loop()
{
    velocityOpenloop(5);
}
